#include "dbnet/RTMapperNetInterface.h"
#include "GSLAM/core/GSLAM.h"
#include "opencv2/highgui/highgui.hpp"

using namespace GSLAM;

struct RTMVWriter
{
    RTMVWriter(std::string file)
        :ofs(file,std::ios::binary){}

    bool feedJPEG(sv::Svar fr){
        sv::Svar image=fr["image"];
        sv::Svar lat = fr["lat"];
        sv::Svar lng = fr["lng"];
        sv::Svar alt = fr["alt"];
        sv::SvarBuffer payload(0);

        if(image.is<GImage>()){
            std::vector<uchar> buf;
            //cv::imencode(".jpg",cv::Mat(image.as<GImage>()),buf);
            //payload=sv::SvarBuffer(buf.data(),buf.size()).clone();
        }
        else if(image.is<sv::SvarBuffer>()){// should be the jpeg encoded buffer
            payload=image.as<sv::SvarBuffer>();
        }
        else if(image.is<std::string>()){
            payload=sv::SvarBuffer::load(image.as<std::string>());
        }

        RTMapperNetHeader header;
        header.payload_size=payload.size();
        header.timestamp=fr.get("timestamp",0.);
        header.lat=fr.get("lat",0.);
        header.lng=fr.get("lng",0.);
        header.alt=fr.get("alt",0.);
        header.H=fr.get("H",0.);
        header.HDOP_H=fr.get("HDOP_H",0.);
        header.HDOP_V=fr.get("HDOP_V",0.);
        header.uav_roll=fr.get("uav_roll",0.);
        header.uav_pitch=fr.get("uav_pitch",0.);
        header.uav_yaw=fr.get("uav_yaw",0.);
        header.cam_roll=fr.get("cam_roll",0.);
        header.cam_pitch=fr.get("cam_pitch",0.);
        header.cam_yaw=fr.get("cam_yaw",0.);
        header.video_codec=VIDEOCODEC_JPEG;

        ofs.write((char*)&header,sizeof(header));
        ofs.write((char*)payload._ptr,payload.size());

        return ofs.good();
    }

    std::ofstream ofs;
};

REGISTER_SVAR_MODULE(RTMVWriter){
    sv::Class<RTMVWriter>("RTMVWriter")
            .unique_construct<std::string>()
            .def("feedJPEG",&RTMVWriter::feedJPEG);

}
